#include <ErraticModel.hpp>

Erratic::Erratic(){
	init(INF,INF,INF,INF,INF);
	
	maximum_linear_velocity = 0.3f;
	maximum_angular_velocity = 1.0f;
	maximum_linear_acc = 0.3;
	maximum_angular_acc = 1.0;
}

Erratic::Erratic(double cx, double cy, double ct, double cvl, double cvt){
	
	init(cx,cy,ct,cvl,cvt);
	
	maximum_linear_velocity = 0.3f;
	maximum_angular_velocity = 1.0f;
	maximum_linear_acc = 0.3;
	maximum_angular_acc = 1.0;
}

void Erratic::init(double cx, double cy, double ct, double cvl, double cvt){
	_x = cx;
	_y = cy;
	_t = ct;
	_lv = cvl;
	_av = cvt;
	//ROS_INFO("INIT %lf %lf",_x,_y);
}

double Erratic::x( double cx){ 
	_x = ( cx!= INF ) ? cx : _x ;
	return _x; 
}

double Erratic::y( double cy){
	_y = ( cy!= INF ) ? cy : _y ;
	return _y; 
}

double Erratic::theta( double ctheta ){
	_t = ( ctheta != INF ) ? ctheta : _t;
	return _t;
}

double Erratic::lv( double cvx ){
	_lv = ( cvx != INF ) ? cvx : _lv;
	return _lv;
}

double Erratic::av( double cvt ){
	_av = ( cvt != INF ) ? cvt : _av;
	return _av;
}

double Erratic::max_lin_velocity(){ return maximum_linear_velocity; 	}
double Erratic::max_ang_velocity(){ return maximum_angular_velocity;	}
double Erratic::max_lin_acc(){ return maximum_linear_acc; 	}
double Erratic::max_ang_acc(){ return maximum_angular_acc;	}
